/**
* \brief 
* \author pengcheng (pengcheng@yslrpch@126.com)
* \date 2020-05-23
* \attention CopyrightÃ‚Â©ADC Technology(tianjin)Co.Ltd
* \attention Refer to COPYRIGHT.txt for complete terms of copyright notice
*/


#include <cmw_app/front_vision.h>
#include "common/project_root.h"

namespace adc
{


FrontVisionApp::FrontVisionApp(const std::string &left_camera_params)
{
    cv::Mat K_l, D_l, R_l, P_l;
    cv::Size size_l;
    adc::StereoSgmApp::ReadParams(left_camera_params,K_l, D_l, P_l, R_l, size_l);
    cv::initUndistortRectifyMap(K_l, D_l, R_l, P_l, size_l, CV_32FC1, map_l_x_, map_l_y_);

}

FrontVisionApp::FrontVisionApp(const std::string &stereo_config_file, const std::string &detectiion_config_file, const std::string &mark_ground_config)
{
    // (void) detectiion_config_file;
    InitConfig(stereo_config_file, detectiion_config_file, mark_ground_config);
}

void FrontVisionApp::InitConfig(const std::string &stereo_config_file, const std::string &detection_config_fie, const std::string &mark_ground_config)
{
    //(void) detection_config_fie;
    if(stereo_config_file.length() > 0) stereo_app_ptr_ = std::make_shared<adc::StereoSgmApp>(stereo_config_file, 40);
    if(detection_config_fie.length() > 0) detection_app_ptr_ = std::make_shared<adc::DetectionApp>(detection_config_fie,20);
    if(mark_ground_config.length()> 0) mark_ground_detection_ptr_ = std::make_shared<adc::DetectionCenternet>(mark_ground_config, 30);
    
}

void FrontVisionApp::SetTask(std::string &config_file, TASK task, float hz)
{
    switch (task)
    {
    case TASK::STEREO :
        if(config_file.length()>0) 
        {
            std::string config =  adc::RootPath::GetAbsolutePath(config_file);
            stereo_app_ptr_ = std::make_shared<adc::StereoSgmApp>(config, hz);
            LOG(INFO)<<"Front app Set Stereo task using: "<<config;
        }
        else
        {
            LOG(ERROR)<<"Front app Set Stereo task Faile, the config file is empty";
        }
        break;
    case TASK::OBSTACLE_DETECTION_YOLO :
        if(config_file.length()>0)
        {
            std::string config =  adc::RootPath::GetAbsolutePath(config_file);
            detection_app_ptr_ = std::make_shared<adc::DetectionApp>(config, hz);
            LOG(INFO)<<"Front app Set Obstacle detection yolo v3 task using: "<<config;

        }
        else
        {
            LOG(ERROR)<<"Front app Set Obstacle detection yolo v3 task Faile, the config file is empty";
            
        }
        break;
    case TASK::MARK_GROUND_DETECTION_CENTERNET:
        if(config_file.length()>0)
        {
            std::string config =  adc::RootPath::GetAbsolutePath(config_file);
            mark_ground_detection_ptr_ = std::make_shared<adc::DetectionCenternet>(config, hz);
            LOG(INFO)<<"Front app Set Mark  ground detction center net task using: "<<config;
            
        }
        else
        {
            LOG(ERROR)<<"Front app Set Mark  ground detction center net task Faile, the config file is empty";
            
        }
        break;
    case TASK::CCTSDB_DETECTION_CENTERNET:
        if(config_file.length()>0)
        {
            std::string config =  adc::RootPath::GetAbsolutePath(config_file);
            cctsdb_detection_ptr_ = std::make_shared<adc::DetectionCenternet>(config, hz);
            LOG(INFO)<<"Front app Set CCTSDB detction center net task using: "<<config;
        }
        else
        {
            LOG(ERROR)<<"Front app Set CCTSDB detction center net task Faile, the config file is empty";
            
        }
       break;
    default:
        LOG(ERROR)<<"Front vison app, set task fiale";
        break;
    }
}

void FrontVisionApp::SetStereoPointCloudCallback(PointCloudCallback callback)
{
    if(stereo_app_ptr_)
    {
        stereo_app_ptr_->SetPointCloudCallback(callback);
    }
}

void FrontVisionApp::SetStereoPointColorCallback(PointColorCallback callback)
{
    if(stereo_app_ptr_)
    {
        stereo_app_ptr_->SetPointColorCallback(callback);
    }
}

void FrontVisionApp::SetStereoPointAndColorCallback(PointAndColorCallback callback)
{
    if(stereo_app_ptr_)
    {
        stereo_app_ptr_->SetPointAdnColorCallback(callback);
    }
}

void FrontVisionApp::SetStereoPointCloudMatCallback(PointCloudMatCallback callback)
{
    if(stereo_app_ptr_)
    {
        stereo_app_ptr_->SetPointCloudMatCallback(callback);
    }
}
void FrontVisionApp::SetDetectionObjectCallback(DetectionCallback callback)
{
   if(detection_app_ptr_)
   {
       detection_app_ptr_->SetDetectionCallback(callback);
   }
}


void FrontVisionApp::SetMarkGroundDetectionCallback(MarkGroundDetectionCallback callback)
{
    if(mark_ground_detection_ptr_)
    {
        mark_ground_detection_ptr_->SetDetectionCallback(callback);
    }
}

void FrontVisionApp::SetMarkGroundDetectionWithImgCallback(MarkGroundDetectionWithSrcCallback callback)
{
    if(mark_ground_detection_ptr_)
    {
        mark_ground_detection_ptr_->SetDetectionImgCallback(callback);
    }
}

 void  FrontVisionApp::SetCCTSDBDetectionCallback(CCTSDBDetectionCallback callback)
 {
     if(cctsdb_detection_ptr_)
     {
         cctsdb_detection_ptr_->SetDetectionCallback(callback);
     }
 }


void FrontVisionApp::SetCCTSDBDetectionWithImgCallback(CCTSDBDetectionWithSrcCallback callback)
{
    if(cctsdb_detection_ptr_)
    {
        cctsdb_detection_ptr_->SetDetectionImgCallback(callback);
    }
}
    

void FrontVisionApp::SetImage(cv::Mat &img, TimeType &time)
{
    if(detection_app_ptr_)
    {
        detection_app_ptr_->CacheImages(img.clone(), time);
    }
    
    if(mark_ground_detection_ptr_)
    {
        mark_ground_detection_ptr_->CacheImages(img, time);
    }

    if(cctsdb_detection_ptr_)
    {
        cctsdb_detection_ptr_->CacheImages(img, time);
    }
    
}

void FrontVisionApp::SetImages(cv::Mat &left, cv::Mat &right,  TimeType &time)
{
    
    if(stereo_app_ptr_)
    {    
        stereo_app_ptr_->CacheImages(left, right, time);
    }
    cv::Mat img = left.clone();
    if(!map_l_x_.empty())
    {
        cv::remap(img, img, map_l_x_, map_l_y_, cv::INTER_LINEAR);

    }
    if(detection_app_ptr_)
    {
        detection_app_ptr_->CacheImages(img,time);
    }
    if(mark_ground_detection_ptr_)
    {
        mark_ground_detection_ptr_->CacheImages(img, time);
    }
    if(cctsdb_detection_ptr_)
    {
        cctsdb_detection_ptr_->CacheImages(img, time);
    }
    
}

void FrontVisionApp::Ready()
{
    if(stereo_app_ptr_)
    {
        stereo_app_ptr_->Run();
    }
    if(detection_app_ptr_)
    {
        detection_app_ptr_->Run();
    }
    if(mark_ground_detection_ptr_)
    {
        mark_ground_detection_ptr_->Run();
    }
    if(cctsdb_detection_ptr_)
    {
        cctsdb_detection_ptr_->Run();
    }
}

}
